#include "FrequencyDirectSolver.h"

#include "Utility/Logging/SGLogger.h"

#include "DataStructure/Input/Solver/Subcase.h"
#include "Utility/Algebra/Factory.h"
#include "Utility/Algebra/LinearSolver.h"

#include "DBManager/IService/Input/Parameter/IParameterService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"

#include "SGFem/Task/Kernel/MModel.h"

using SG::Algebra::SpMat_t;
using SG::Algebra::ComplexSpMat_t;
using namespace SG::Algebra;
using SG::DataStructure::Common::Status;
using SG::DataStructure::Common::Real;
using SG::DataStructure::FEM::Project;
using namespace SG::DBManager;

void FrequencyDirectSolver::Solve (const DBServiceFactorySP dbServiceFactory, const Project& project, int subcaseId)
{
    // 查询工况参数
    auto                            para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    SG::DataStructure::FEM::Subcase subcase (para);
    Kernel::MModel           model (SG::DataStructure::FEM::SOLType::SOL_SEDFREQ, dbServiceFactory);
    
    // 频率（起始频率,dw,频率步数）
    std::vector<Real> w;
    if(Status::STATUS_Failure == model.QueryFrenqencePoint (subcase.GetFrequencyId (), _OUT w))
    {
        return;
    }
    

    model.Initialize ({subcaseId});
    model.CalcMatrixEquation (subcaseId);

    // 动刚度矩阵
    ComplexSpMat_t K_complex;
    model.ComputeDynaStiffness (_OUT K_complex);

    // 质量阵M、粘滞阻尼阵C
    SpMat_t        M, C;                                         
    model.ComputeMassMatrix (M);      
    ComplexSpMat_t M_complex = M.cast<Complex_t> ();
    model.ComputeDampingMatrix (C);   
    ComplexSpMat_t C_complex = C.cast<Complex_t> ();


    ComplexVec_t          P_k;                                             // theta[k]圆频率下的外力
    Vec_t          u;
    Vec_t          v;                              
    ComplexVec_t   displacement;                                    // 位移
    ComplexSpMat_t K_hat;                                           // 动刚度阵 K_hat = -θ² * M + i * θ * C + K
    
    std::string solver_type = "TPardiso<Complex_t>";
    auto        solver      = reinterpret_cast<TLinearSolver<Complex_t>*> (SG::Base::Factory::GetInstance ()->Produce (solver_type));
    

    // 计算动力载荷的空间分布
    model.ComputeFValue (subcaseId);

    for (size_t i = 0; i < w.size(); ++i)
    {
        auto theta = 2.0 * M_PI * w[i];

        // 求解K_hat * displacement = P_k
        K_hat = -std::pow (theta, 2) * M_complex + Complex_t (0.0, theta) * C_complex + K_complex;
        
        solver->compute (K_hat);
        // 当前频率的载荷向量
        model.ComputeFreqForce (w[i], u, v, _OUT P_k);
        solver->solve (P_k, displacement);
        model.SetDisplacement (displacement, subcaseId, w[i]);
    }

    SGInfo << "FrequencyAnalysis Solve fini  \n";
}